#include "maptcpsocket.h"

#include <QJsonDocument>
#include <QDebug>
#include <QThread>
#include <QJsonDocument>
#include <QLabel>
#include <QBuffer>
#include <QMessageBox>
#include <QHostAddress>
#include <QTimer>


#include<fstream>
#include<iostream>

mapTcpSocket::mapTcpSocket(QObject *parent)
    : QObject{parent}
{

}

mapTcpSocket::~mapTcpSocket()
{
    if(m_socket != NULL)
    {
        m_socket->disconnectFromHost();
        m_socket->deleteLater();
        m_socket = NULL;
    }
}

bool mapTcpSocket::connectToHost(QString ip, int port)
{
    qDebug()<<"connectToHost"<<QThread::currentThreadId();
    qDebug()<<"mapTcpSocket::connectToHost="<<ip<<port;
    m_Ip = ip;m_Port = port;
    m_ByteDataTcp.clear();
    emit sig_Loger("mapTcpSocket::connectToHost="+ip);
    if(ip.isEmpty())
    {
        if(m_socket != NULL)
        {
            m_socket->disconnectFromHost();
            m_socket->deleteLater();
            m_socket = NULL;
        }
        return false;
    }
    else
    {
        if(NULL != m_socket)
        {
            m_socket->disconnectFromHost();
            m_socket->deleteLater();
            m_socket = NULL;
        }
        m_socket = new QTcpSocket(this);
        m_socket->connectToHost(QHostAddress(ip),port);
        if(!m_socket->waitForConnected(1000))
        {
            qDebug()<<QString(u8"连接服务器失败!\n");
            return false;
        }
        while (m_socket->bytesAvailable() > 0) {
            m_socket->readAll();
        }
        connect(m_socket, SIGNAL(readyRead()), this, SLOT(slotTcpRevData()));
        return true;
    }

}

void mapTcpSocket::slot_getCreatorMapData(QString Msg)
{
    if(Msg == "startGetNewCteatorMapImage")
    {
        is_Get = true;
        QString send_data = QString("R\"({\"op\": \"subscribe\",\"topic\": \"%1\",\"throttle_rate\": 1000000});").arg(g_RobotCreateMap);
        sendData(send_data);
    }
    else if(Msg == "stopGetNewCteatorMapImage")
    {
        is_Get = false;
        QString send_data = QString("R\"({\"op\": \"unsubscribe\",\"topic\": \"/map\"})\";").arg(g_RobotCreateMap);
        sendData(send_data);
    }

}


void mapTcpSocket::sendData(QString Msg)
{
    qDebug()<<u8"mapTcpSocketsendData:"<<Msg;
    emit sig_Loger(QString(u8"mapTcpSocketsendData:%1").arg(Msg));

    if(m_socket == NULL)
        return;
    QString sendMsg = Msg;
    char sendMsgChar[300] = {0};
    strcpy(sendMsgChar, sendMsg.toStdString().c_str());
    int sendRe = m_socket->write(sendMsgChar, sendMsg.length());

    m_socket->waitForBytesWritten(1000);
    m_socket->flush();
    if(sendRe == -1)
    {
        qDebug()<<QString(u8"向服务端发送数据失败！");
        connectToHost(m_Ip,m_Port);

    }else
    {
        qDebug()<<QString(u8"向服务端发送数据成功！");
    }
    m_ByteDataTcp.clear();
}

void mapTcpSocket::slotTcpRevData()
{
    while (m_socket->bytesAvailable() > 0) {
        m_ByteDataTcp += m_socket->readAll();
    }
    //qDebug()<<"mapTcpSocketslotTcpRevData="<<m_ByteDataTcp.length();
    QString  showQstr = QString(m_ByteDataTcp);

    if(!showQstr.startsWith("{"))//找JSON头
    {
        qDebug()<<"showQstr.length"<<showQstr.length();
        if(showQstr.indexOf("}{") != -1)
        {
            showQstr.remove(0,showQstr.indexOf("}{")+1);
            m_ByteDataTcp.remove(0,showQstr.indexOf("}{")+1);
        }
    }

    if(showQstr.startsWith("{") && showQstr.indexOf("}{") != -1)
    {
        showQstr = showQstr.left(showQstr.indexOf("}{")+1);
        //qDebug()<<"m_ByteDataTcp.length()"<<showQstr.length()<<m_ByteDataTcp.length();
    }

    if(showQstr.startsWith("{") && showQstr.endsWith("}"))
    {
        m_ByteDataTcp.remove(0,showQstr.length());
        QByteArray arr = showQstr.toUtf8();
        if(arr.isEmpty())
            return;
        QJsonParseError err;
        QJsonDocument mdocument = QJsonDocument::fromJson(arr, &err);
        if(!mdocument.isEmpty())   // 如果加载json数据失败则打印失败信息
        {
            QVariantMap dataMap;
            dataMap=mdocument.toVariant().toMap();
            qDebug()<< "mapTcpSocketslotTcpRevData()" <<dataMap["topic"].toString();
            if(dataMap["topic"].toString() == g_RobotCreateMap)//建图数据
            {
                QVariantMap magMap = dataMap["msg"].toMap();
                QVariantMap infoMap = magMap["info"].toMap();
                g_map_resolution = infoMap["resolution"].toDouble();
                QVariantMap originMap = infoMap["origin"].toMap();
                QVariantMap orientationMap = originMap["orientation"].toMap();
                QVariantMap positionMap = originMap["position"].toMap();
                g_map_origin[0] = positionMap["x"].toDouble();
                g_map_origin[1] = positionMap["y"].toDouble();

                QVariantList dataList = magMap["data"].toList();

                qDebug()<<"lenth="<<infoMap["width"].toDouble()<<infoMap["height"].toDouble()<<dataList.length() <<g_map_origin[0]<<g_map_origin[1]<<g_map_resolution;
                DataToPgm(dataList,infoMap["width"].toDouble(),infoMap["height"].toDouble());

                if(is_Get)
                {
                    QString send_data = QString("R\"({\"op\": \"unsubscribe\",\"topic\": \"/map\"})\";").arg(g_RobotCreateMap);
                    sendData(send_data);
                    QTimer::singleShot(1000,this,[this](){
                        QString send_data = QString("R\"({\"op\": \"subscribe\",\"topic\": \"%1\",\"throttle_rate\": 1000000});").arg(g_RobotCreateMap);
                        sendData(send_data);
                    });

                }
            }
        }
    }
}


void mapTcpSocket::DataToPgm(QVariantList dataList, int width, int height)
{
    qDebug()<<"mapTcpSocketDataToPgm"<<width<<height<<dataList.length();
    int size = dataList.length();
    int8_t *data = new int8_t[size];
    int offset = 0;
    for (const QVariant &variant : dataList) {
        data[offset++] = variant.toInt();
    }
    displayPGM(data,width,height,255);
    // 清理分配的内存
    delete[] data;
}

void mapTcpSocket::displayPGM(int8_t *data, int width, int height, int maxValue)
{
    int threshold_occupied = 65;
    int threshold_free = 25;

    std::string mapdatafile = g_strExeRoot.toStdString()+"/createMap.pgm";
    FILE* out = fopen(mapdatafile.c_str(), "w");
    if (!out)
    {
        return;
    }
    fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
            g_map_resolution, width, height);
    for(unsigned int y = 0; y < height; y++) {
        for(unsigned int x = 0; x < width; x++) {
            unsigned int i = x + (height - y - 1) * width;
            if (data[i] >= 0 && data[i] <= threshold_free) { // [0,free)
                fputc(254, out);
            } else if (data[i] >= threshold_occupied) { // (occ,255]
                fputc(000, out);
            } else { //occ [0.25,0.65]
                fputc(205, out);
            }
        }
    }

    fclose(out);
    QPixmap SavePixmap(g_strExeRoot+"/createMap.pgm");
    emit sig_mapTopicPgm(SavePixmap);

    return;
    QImage image(width, height, QImage::Format_Grayscale8);
    // 填充索引颜色表
    QVector<QRgb> colorTable;
    for (int i = 0; i < 256; i++) {
        colorTable.push_back(qRgb(i, i, i));
    }
    image.setColorTable(colorTable);

    // 将数据复制到QImage
    for (int y = 0; y < height; y++) {
        for (int x = 0; x < width; x++) {
            //image.setPixel(x, y, data[y * width + x]);
            // 确保数据在0-255范围内
            uint8_t value = qBound(uint8_t(0), static_cast<uint8_t>(data[y * width + x]), uint8_t(255));
            image.setPixel(x, y, qRgb(value, value, value));
        }
    }
    // 转换为PGM格式
    QByteArray pgmHeader = QString("P5\n%1 %2\n%3\n").arg(width).arg(height).arg(maxValue).toLatin1();
    QFile file("output.pgm");
    file.open(QIODevice::WriteOnly);
    file.write(pgmHeader);
    file.write((const char*)data, width * height);
    file.close();
    // 显示图像
    QPixmap pixmap = QPixmap::fromImage(image);
    emit sig_mapTopicPgm(pixmap);
}

void mapTcpSocket::savePGM(const std::string &filename, int8_t *data, int width, int height, int max_value)
{
    std::ofstream file(filename, std::ios::binary);
    if (!file) {
        std::cerr << "Cannot open file " << filename << " for writing." << std::endl;
        return;
    }
    file << "P5" << std::endl; // Portable PGM format
    file << width << " " << height << std::endl;
    file << max_value << std::endl;
    // Write the pixel data
    file.write(reinterpret_cast<const char*>(data), width * height);
    file.close();
}
